package com.hitqz.robot.driver.zmq.protocol.component;

import com.example.tutorial.AddressBookProtos;
import com.google.protobuf.ByteString;
import com.hitqz.robot.driver.zmq.protocol.message.ProtocolMessage;
import lombok.extern.slf4j.Slf4j;
import navconnector.ChassisXc;
import navconnector.ChassisYichun;
import navconnector.NavconnectorBase;

import java.util.List;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;

/**
 * 机器人通讯组件
 *
 * @author xupeikun
 */
@Slf4j
public class InDoorRobotComTool {

    private final ExecutorService executor = Executors.newSingleThreadExecutor();

    private String backName;

    public InDoorRobotComTool(String backName) throws Exception {
        this.backName = backName;
        try {
            initServer();
        } catch (Exception e) {
            e.printStackTrace();
            throw new Exception(e);
        }
    }


    private void initServer() throws Exception {
        executor.execute(new SendThread(backName));
    }

    // 发送数据
    private static void sendMsg(String address, String type, byte[] data) {
        MsgQueue.addOne(new ProtocolMessage(address, type, data));
    }

    /**
     * 发送void数据对象消息  request 里面不包括任何对象
     */
    public static void voidCommand(String address, Integer seq, Integer objId) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.Void message = AddressBookProtos.Void.newBuilder()
                .setHeader(header)
                .build();
        sendMsg(address, "navconnector.Void", message.toByteArray());
    }

    /**
     * 发送Float数据对象消息
     */
    public static void floatCommand(String address, Integer seq, Integer objId, double data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.Float message = AddressBookProtos.Float.newBuilder()
                .setHeader(header)
                .setData(data)
                .build();
        sendMsg(address, "navconnector.Float", message.toByteArray());
    }


    /**
     * 发送PoseStamped数据对象消息
     */
    public static void poseStampedCommand(String address, Integer seq, Integer objId, NavconnectorBase.Pose pose) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.PoseStamped message = AddressBookProtos.PoseStamped.newBuilder()
                .setHeader(header)
                .setPose(pose)
                .build();
        sendMsg(address, "navconnector.PoseStamped", message.toByteArray());
    }

    /**
     * 发送TwistStamped数据对象消息
     */
    public static void twistStampedCommand(String address, Integer seq, Integer objId,
                                           double linearX, double linearY, double linearZ,
                                           double angularX, double angularY, double angularZ) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.TwistStamped message = AddressBookProtos.TwistStamped.newBuilder()
                .setHeader(header)
                .setLinearX(linearX)
                .setLinearY(linearY)
                .setLinearZ(linearZ)
                .setAngularX(angularX)
                .setAngularY(angularY)
                .setAngularZ(angularZ)
                .build();
        sendMsg(address, "navconnector.TwistStamped", message.toByteArray());
    }

    /**
     * 发送int32数据对象消息
     */
    public static void int32Command(String address, Integer seq, Integer objId,int data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.Int32 message = AddressBookProtos.Int32.newBuilder()
                .setHeader(header)
                .setData(data)
                .build();
        sendMsg(address, "navconnector.Int32", message.toByteArray());
    }


    /**
     * 发送int32Array数据对象消息
     */
    public static void int32ArrayCommand(String address, Integer seq, Integer objId,int[] data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.Int32Array.Builder builder = AddressBookProtos.Int32Array.newBuilder();
        for (int i = 0; i < data.length; i++) {
            builder.setData(i,data[i]);
        }
        builder.setHeader(header);
        AddressBookProtos.Int32Array message = builder.build();
        sendMsg(address, "navconnector.Int32Array", message.toByteArray());
    }

    /**
     * 发送String数据对象消息
     */
    public static void stringCommand(String address, Integer seq, Integer objId,String data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.String message = AddressBookProtos.String.newBuilder()
                .setHeader(header)
                .setData(data)
                .build();
        sendMsg(address, "navconnector.String", message.toByteArray());
    }

    /**
     * 发送StringArray数据对象消息
     */
    public static void stringArrayCommand(String address, Integer seq, Integer objId,List<String> data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.StringArray message = AddressBookProtos.StringArray.newBuilder()
                .setHeader(header)
                .addAllData(data)
                .build();
        sendMsg(address, "navconnector.StringArray", message.toByteArray());
    }

    /**
     * 发送FileOperator数据对象消息
     */
    public static void fileOperatorCommand(String address, Integer seq, Integer objId,String data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.FileOperator message = AddressBookProtos.FileOperator.newBuilder()
                .setHeader(header)
                .build();
        sendMsg(address, "navconnector.FileOperator", message.toByteArray());
    }

    public static void fileWithNameOperatorCommand(String address, Integer seq, Integer objId, String name,byte[] data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.FileWithNameOperator message = AddressBookProtos.FileWithNameOperator.newBuilder()
                .setHeader(header)
                .setName(name)
                .setData(ByteString.copyFrom(data))
                .build();
        sendMsg(address, "navconnector.FileWithNameOperator", message.toByteArray());
    }

    /**
     * 发送Sensor数据对象消息 目前仅设置传感器避障状态一个命令
     */
    public static void sensorCommand(String address, Integer seq, int sonar,int antiCollision,int avoidInfrared,
                                     int avoidLidar,int dropProof,int micrometerWaveLidar) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(AddressBookProtos.Sensor.OBJ_ID.SET_SENSOR_AVOID_ENABLE_REQ_VALUE)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.Sensor message = AddressBookProtos.Sensor.newBuilder()
                .setHeader(header)
                //fixme 目前不支持单个设置
                .addSonar(sonar)
                .addAntiCollision(antiCollision)
                .addAvoidInfrared(avoidInfrared)
                .addAvoidLidar(avoidLidar)
                .addDropProof(dropProof)
                .addMicrometerWaveLidar(micrometerWaveLidar)
                .build();
        sendMsg(address, "navconnector.Sensor", message.toByteArray());
    }

    /**
     * 发送GoalList数据对象消息 目前仅命名目标点序列（自由导航）一个命令
     */
    public static void goalListCommand(String address, Integer seq, String taskId, List<AddressBookProtos.NamedPose> data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(AddressBookProtos.GoalList.OBJ_ID.SET_MOVE_BASE_GOAL_LIST_REQ_VALUE)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.GoalList message = AddressBookProtos.GoalList.newBuilder()
                .setHeader(header)
                .setTaskId(taskId)
                .addAllData(data)
                .build();
        sendMsg(address, "navconnector.GoalList", message.toByteArray());
    }

    /**
     * 发送StationArray数据对象消息 目前仅设置导航队列 一个命令
     */
    public static void stationArrayCommand(String address, Integer seq, String taskId, List<AddressBookProtos.Station> data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(AddressBookProtos.StationArray.OBJ_ID.SET_STATION_TASK_REQ_VALUE)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.StationArray message = AddressBookProtos.StationArray.newBuilder()
                .setHeader(header)
                .setTaskId(taskId)
                .addAllStations(data)
                .build();
        sendMsg(address, "navconnector.StationArray", message.toByteArray());
    }

    /**
     * 发送ForwardSSH数据对象消息 目前仅设置SSH 一个命令
     */
    public static void forwardSSHCommand(String address, Integer seq, String remoteUser, String remoteIp,
                                         int remoteSSHPort,int remoteBindPort,int localSSHPort) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(AddressBookProtos.ForwardSSH.OBJ_ID.CMD_FORWARD_SSH_REQ_VALUE)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.ForwardSSH message = AddressBookProtos.ForwardSSH.newBuilder()
                .setHeader(header)
                .setRemoteUser(remoteUser)
                .setRemoteIp(remoteIp)
                .setRemoteSshPort(remoteSSHPort)
                .setRemoteBindPort(remoteBindPort)
                .setLocalSshPort(localSSHPort)
                .build();
        sendMsg(address, "navconnector.ForwardSSH", message.toByteArray());
    }

    /**
     * 发送DelayChecker数据对象消息 目前测试网络延迟 一个命令
     */
    public static void delayCheckerCommand(String address, Integer seq) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(AddressBookProtos.DelayChecker.OBJ_ID.GET_DELAY_REQ_VALUE)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.DelayChecker message = AddressBookProtos.DelayChecker.newBuilder()
                .setHeader(header)
                .setStartStamp(System.currentTimeMillis())
                .build();
        sendMsg(address, "navconnector.DelayChecker", message.toByteArray());
    }

    /**
     * 发送ChassisVoid数据对象消息 目前仅获取门禁使能状态 一个命令
     */
    public static void chassisVoidCommand(String address, Integer seq) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(ChassisYichun.ChassisVoid.OBJ_ID.GET_ACCESS_CTRL_ENABLE_REQ_VALUE)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        ChassisYichun.ChassisVoid message = ChassisYichun.ChassisVoid.newBuilder()
                .setHeader(header)
                .build();
        sendMsg(address, "navconnector.ChassisVoid", message.toByteArray());
    }

    /**
     * 发送chassisInt32数据对象消息
     */
    public static void chassisInt32Command(String address, Integer seq,Integer obj,int data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(obj)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        ChassisYichun.ChassisInt32 message = ChassisYichun.ChassisInt32.newBuilder()
                .setHeader(header)
                .setData(data)
                .build();
        sendMsg(address, "navconnector.ChassisInt32", message.toByteArray());
    }

    /**
     * 发送ChassisInt32Array数据对象消息 目前仅顶升机构控制命令 一个命令
     */
    public static void chassisInt32ArrayCommand(String address,Integer obj, Integer seq,int[] data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(obj)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        ChassisYichun.ChassisInt32Array.Builder builder = ChassisYichun.ChassisInt32Array.newBuilder();
        for (int i = 0; i < data.length; i++) {
            builder.addData(data[i]);
        }
        builder.setHeader(header);
        ChassisYichun.ChassisInt32Array message = builder.build();
        sendMsg(address, "navconnector.ChassisInt32Array", message.toByteArray());
    }


    /**
     * 发送ChassisHeadControlParam数据对象消息 目前仅控制头部
     */
//    public static void chassisHeadControlParamCommand(String address, Integer obj, Integer seq, RobotHeadAngle headAngle) {
//        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
//                .setObj(obj)
//                .setSeq(seq)
//                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
//                .setStamp(System.currentTimeMillis())
//                .build();
//        ChassisYichun.ChassisHeadControlParam.Builder builder = ChassisYichun.ChassisHeadControlParam.newBuilder();
//        builder.setHeadAngles(headAngle.getVerticalAngle())
//                .setLeftAngles(headAngle.getHorizontalAngle())
//                .setRightAngles(0)
//                .setHeadSpeed(0)
//                .setLeftSpeed(0)
//                .setRightSpeed(0)
//                .setMotoValidBit(0);
//        builder.setHeader(header);
//        ChassisYichun.ChassisHeadControlParam message = builder.build();
//        sendMsg(address, "navconnector.ChassisHeadControlParam", message.toByteArray());
//    }

//    public static void chassisXCHeadControlParamCommand(String address, Integer obj, Integer seq, RobotHeadAngle headAngle) {
//        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
//                .setObj(obj)
//                .setSeq(seq)
//                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
//                .setStamp(System.currentTimeMillis())
//                .build();
//        ChassisXc.ChassisXCHeadControlParam.Builder builder = ChassisXc.ChassisXCHeadControlParam.newBuilder();
//        builder.setHeadAngles(headAngle.getVerticalAngle())
//                .setLeftAngles(headAngle.getHorizontalAngle())
//                .setRightAngles(0)
//                .setHeadSpeed(0)
//                .setLeftSpeed(0)
//                .setRightSpeed(0)
//                .setMotoValidBit(0x0f);
//        builder.setHeader(header);
//        ChassisXc.ChassisXCHeadControlParam message = builder.build();
//        sendMsg(address, "navconnector.ChassisXCHeadControlParam", message.toByteArray());
//    }

//    public static void chassisInt32ArrayCommand(String address, Integer obj, Integer seq, int[] data) {
//        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
//                .setObj(obj)
//                .setSeq(seq)
//                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
//                .setStamp(System.currentTimeMillis())
//                .build();
//        ChassisYichun.ChassisInt32Array.Builder builder = ChassisYichun.ChassisInt32Array.newBuilder();
//        for (int i = 0; i < data.length; i++) {
//            builder.addData(data[i]);
//        }
//
//        builder.setHeader(header);
//        ChassisYichun.ChassisInt32Array message = builder.build();
//        sendMsg(address, "navconnector.ChassisInt32Array", message.toByteArray());
//    }

    /**
     * 发送 chassisXCVoid 数据对象消息  request 里面不包括任何对象
     */
    public static void chassisXCVoid(String address, Integer seq, Integer objId) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        ChassisXc.ChassisXCVoid message = ChassisXc.ChassisXCVoid.newBuilder()
                .setHeader(header)
                .build();
        sendMsg(address, "navconnector.ChassisXCVoid", message.toByteArray());
    }


    /**
     * 发送ChassisXCInt32数据对象消息
     */
    public static void chassisXCInt32(String address, Integer seq, Integer objId,int data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(objId)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        ChassisXc.ChassisXCInt32 message = ChassisXc.ChassisXCInt32.newBuilder()
                .setHeader(header)
                .setData(data)
                .build();
        sendMsg(address, "navconnector.ChassisXCInt32", message.toByteArray());
    }

    /**
     * 发送chassisInt32数据对象消息
     */
    public static void chassisInt32KeyValueCommand(String address, Integer seq,Integer obj,int key,int value) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(obj)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        ChassisYichun.ChassisInt32KeyValue message = ChassisYichun.ChassisInt32KeyValue.newBuilder()
                .setHeader(header)
                .setKey(key)
                .setValue(value)
                .build();
        sendMsg(address, "navconnector.ChassisInt32KeyValue", message.toByteArray());
    }


    public static void cmdMotorCommand(String address, Integer seq,Integer obj,int id,int cmd,int val) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(obj)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.MotorCmd message = AddressBookProtos.MotorCmd.newBuilder()
                .setHeader(header)
                .setId(id)
                .setCmd(cmd)
                .setVal(val)
                .build();
        sendMsg(address, "navconnector.MotorCmd", message.toByteArray());
    }


    public static void cmdIoCommand(String address, Integer seq,Integer obj,int id,int cmd,int val) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(obj)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.IoCmd message = AddressBookProtos.IoCmd.newBuilder()
                .setHeader(header)
                .setId(id)
                .setCmd(cmd)
                .setVal(val)
                .build();
        sendMsg(address, "navconnector.IoCmd", message.toByteArray());
    }


    public static void yamlFileCommand(String address, Integer seq,Integer obj,String path,String data) {
        NavconnectorBase.Header header = NavconnectorBase.Header.newBuilder()
                .setObj(obj)
                .setSeq(seq)
                .setAck(NavconnectorBase.Header.ACK_SEQ.DEFAULT_ACK_SEQ_VALUE)
                .setStamp(System.currentTimeMillis())
                .build();
        AddressBookProtos.YamlFile message = AddressBookProtos.YamlFile.newBuilder()
                .setHeader(header)
                .setYamlPath(path)
                .setData(data)
                .build();
        sendMsg(address, "navconnector.YamlFile", message.toByteArray());
    }

}
